#include "mcnamara.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "driver/mcpwm_prelude.h"

#ifndef M_PI
#define M_PI 3.141592653589793f
#endif

int debug = 0;  // 调试模式开关，默认关闭

/**
 * @brief 根据速度和偏转角计算四个轮子的速度 (麦轮)
 * 
 * speed的输入范围：±PWM_MOTOR_MAX_VALUE
 */
void set_deflection(int speed, int deflection, int *l1, int *l2, int *r1, int *r2)
{
    /*
            90
        180--丨--0
            270
    */
    if (speed > PWM_MOTOR_MAX_VALUE) speed = PWM_MOTOR_MAX_VALUE;
    if (speed < 0)   speed = 0;

    double rad2deg = M_PI / 180.0;
    double vx = speed * cos(deflection * rad2deg);
    double vy = speed * sin(deflection * rad2deg);

    *l1 = (int)(vy + vx);
    *l2 = (int)(vy - vx);
    *r1 = (int)(vy - vx);
    *r2 = (int)(vy + vx);
}



/**
 * @brief 根据速度、偏转角和旋转率计算四个轮子的速度 
 * 
 * @param speed speed的输入范围：±PWM_MOTOR_MAX_VALUE
 * @param deflection 偏转角度 (0-360)
 * @param rate 旋转率 (±最大建议100)
 * @param l1 左前轮输出速度
 * @param l2 左后轮输出速度
 * @param r1 右前轮输出速度
 * @param r2 右后轮输出速度
 */
void set_deflection_rate(float speed, float deflection, float rate, float *l1, float *l2, float *r1, float *r2)
{
    if (speed > PWM_MOTOR_MAX_VALUE) speed = PWM_MOTOR_MAX_VALUE;
    if (speed < 0)   speed = 0;

    // 将ROS2坐标系转换为底盘坐标系
    // ROS2: 前进=x正，左移=y正
    // 底盘: 前进=90度，左移=180度
    double rad2deg = M_PI / 180.0;
    deflection = fmod(deflection + 90.0, 360.0);  // 将ROS2坐标系旋转90度以匹配底盘坐标系
    double vx = speed * cos(deflection * rad2deg);
    double vy = speed * sin(deflection * rad2deg);
    double vp = -rate * (0.117 + 0.132) / 2.0;  // 轮距117mm, 轴距132mm，单位m

    // 麦轮速度合成
    *l1 = (float)(vy + vx - vp);
    *l2 = (float)(vy - vx + vp);
    *r1 = (float)(vy - vx - vp);
    *r2 = (float)(vy + vx + vp);
}



/**
 * 控制机器人向前移动
 * 
 * @param speed 移动速度
 * 
 * 示例:
 *     move_forward(200);  // 以速度200向前移动
 */
void move_forward(int speed) 
{
    // 调用set_deflection函数获取四个电机的值
    int l1, l2, r1, r2;
    set_deflection(speed, 90, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| w |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}


/**
 * 控制机器人向后移动
 * 
 * @param speed 移动速度
 * 
 * 示例:
 *     move_backward(150);  // 以速度150向后移动
 */
void move_backward(int speed) 
{
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，270度表示向后
    set_deflection(speed, 270, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| x |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}



/**
 * 控制机器人向左移动
 * 
 * @param speed 移动速度
 * 
 * 示例:
 *     move_left(180);  // 以速度180向左移动
 */
void move_left(int speed) {
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，180度表示向左
    set_deflection(speed, 180, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| a |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}


/**
 * 控制机器人向右移动
 * 
 * @param speed 移动速度
 * 
 * 示例:
 *     move_right(180);  // 以速度180向右移动
 */
void move_right(int speed) {
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，0度表示向右
    set_deflection(speed, 0, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| d |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}



/**
 * 控制机器人向左旋转
 * 
 * @param speed 旋转速度
 * 
 * 示例:
 *     rotate_left(200);  // 以速度200向左旋转
 */
void rotate_left(int speed) {
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，180度表示向左旋转
    set_deflection(speed, 180, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| R |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", -l2, abs(r2));
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1);
    PwmMotor_Set_Speed(MOTOR_ID_M2, -l2);    // 反向旋转
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1);
    PwmMotor_Set_Speed(MOTOR_ID_M4, abs(r2)); // 确保正值
}

#include <stdio.h>
#include <stdlib.h>  // 用于abs()函数

// 假设这些已在其他地方定义
extern int debug;
extern void set_deflection(int speed, int angle, int* l1, int* l2, int* r1, int* r2);
extern void Ctrl_Muto(int channel, int value);


/**
 * 控制机器人向右旋转
 * 
 * @param speed 旋转速度
 * 
 * 示例:
 *     rotate_right(200);  // 以速度200向右旋转
 */
void rotate_right(int speed) {
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，0度表示向右旋转
    set_deflection(speed, 0, &l1, &l2, &r1, &r2);
    
    // 调试模式下打印信息
    if (debug == 1) {
        printf("L1:%4d| e |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", abs(l2), -r2);
    }
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1);
    PwmMotor_Set_Speed(MOTOR_ID_M2, abs(l2));  // 确保l2为正值
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1);
    PwmMotor_Set_Speed(MOTOR_ID_M4, -r2);      // r2反向旋转
}



/**
 * 控制机器人向左前方对角线移动
 * @param speed 移动速度
 */
void move_diagonal_left_front(int speed) {
    int l1, l2, r1, r2;
    set_deflection(speed, 135, &l1, &l2, &r1, &r2);
    
    if (debug == 1) {
        printf("L1:%4d| q |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}

/**
 * 控制机器人向左后方对角线移动
 * @param speed 移动速度
 */
void move_diagonal_left_back(int speed) {
    int l1, l2, r1, r2;
    set_deflection(speed, 225, &l1, &l2, &r1, &r2);
    
    if (debug == 1) {
        printf("L1:%4d| z |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}

/**
 * 控制机器人向右前方对角线移动
 * @param speed 移动速度
 */
void move_diagonal_right_front(int speed) {
    int l1, l2, r1, r2;
    set_deflection(speed, 45, &l1, &l2, &r1, &r2);
    
    if (debug == 1) {
        printf("L1:%4d| e |R1:%-4d\n", l1, r1);
        printf("L2:%4d|   |R2:%-4d\n\n", l2, r2);
    }
    
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}

/**
 * 控制机器人向右后方对角线移动
 * @param speed 移动速度
 */
void move_diagonal_right_back(int speed) {
    int l1, l2, r1, r2;
    set_deflection(speed, 315, &l1, &l2, &r1, &r2);
    
    if (debug == 1) {
        printf("L1:=%4d| c |R1:=%-4d\n", l1, r1);
        printf("L2:=%4d|   |R2:=%-4d\n\n", l2, r2);
    }
    
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}



/**
 * 控制机器人漂移运动
 * 
 * @param speed 移动速度
 * @param deflection 偏转角度(0-360度)
 * @param rate 旋转率
 * 
 * 示例:
 *     drifting(200, 90, 50);  // 向前漂移
 * 
 * 角度参考:
 *     180
 *       |
 * 270--+--90
 *       |
 *       0
 */
void drifting(int speed, int deflection, int rate) {
    int l1, l2, r1, r2;
    
    // 获取四个电机的值，考虑偏转角度和旋转率
    set_deflection_rate(speed, deflection, rate, &l1, &l2, &r1, &r2);
    
    // 控制电机
    PwmMotor_Set_Speed(MOTOR_ID_M1, l1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M2, l2 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M3, r1 + 0);
    PwmMotor_Set_Speed(MOTOR_ID_M4, r2 + 0);
}
